#include "Integrateur.hpp"

#include "Utils/debugstreams.hpp"

#include <boost/numeric/ublas/vector_expression.hpp>

//#include <limits>

using namespace std;
using namespace CalculIntegral;
using namespace boost::numeric::ublas;

Integrateur::Integrateur( const Fonction& fct,
			  const Point& valeur,
			  const double& taille )
  : FonctionPtr( fct ), ValeurInitiale( valeur ), TailleEtape( taille )
{}

Integrateur::~Integrateur()
{}

void Integrateur::setFonction( const Fonction& fct )
{
  FonctionPtr = fct;
}

void Integrateur::setValeurInitiale( const Point& valeur )
{
  ValeurInitiale = valeur;
}

const Point& Integrateur::valeurInitiale() const
{
  return ValeurInitiale;
}

void Integrateur::setTailleEtape( const double& taille )
{
  TailleEtape = taille;
}

const double& Integrateur::tailleEtape() const
{
  return TailleEtape;
}

Point Integrateur::operator()( const Point& inf,
			       const Point& sup ) const
{
  Point Valeur( ValeurInitiale );

  double NbrEtapes( nombreEtapes( inf, sup ) );

  VecteurBase Increments( 0. );

  Increments = ( sup - inf ) / NbrEtapes;

  Point arguments( inf );

  do
    {
      Valeur = rungeKutta4( arguments, Increments, Valeur );
    }
  while ( inner_prod( arguments - sup , arguments - sup ) >
	  pow( TailleEtape, 2 ) );

  return Valeur;
}

double Integrateur::nombreEtapes( const Point& inf,
				  const Point& sup ) const
{
  unsigned dim( inf.size() ), i;

  double MaxDiff( -1. );

  VecteurBase diff( dim );

  diff = sup - inf;

  for ( i = 0; i < dim; i++ )
    {
      if ( abs( diff[ i ] ) > MaxDiff )
	MaxDiff = abs( diff[ i ] );
    }

  return ceil( MaxDiff / TailleEtape );
}

Point Integrateur::rungeKutta4( Point& variables,
				const Point& increments,
				const Point& valeur) const
{
  unsigned nLignes( FonctionPtr.nbrLignes() ),
    nColonnes( FonctionPtr.nbrColonnes() );
  
  MatriceBase k1( nLignes, nColonnes ), k2( nLignes, nColonnes ),
    k3( nLignes, nColonnes ), k4( nLignes, nColonnes );

  VecteurBase vars, vals;
  
  k1 = FonctionPtr( valeur, variables );
  vals = prod( k1, increments );
  vals *= .5;
  vals += valeur;
  vars = .5 * increments;
  vars += variables;
  k2 = FonctionPtr( vals, vars/*valeur + .5 * prod( k1, increments ),
				variables + .5 * increments*/ );
  vals = prod( k2, increments );
  vals *= .5;
  vals += valeur;
  vars = .5 * increments;
  vars += variables;
  k3 = FonctionPtr( vals, vars/*valeur + .5 * prod( k2, increments ),
				variables + .5 * increments*/ );
  vals = prod( k3, increments );
  vals += valeur;
  vars = increments;
  vars += variables;
  k4 = FonctionPtr( vals, vars/*valeur +  prod( k3, increments ),
				variables +  increments*/ );

  variables += increments;

  vals = prod( k1 + 2. * k2 + 2. * k3 + k4, increments );
  vals *= 1. / 6.;
  
  vals += valeur;

  return vals;
}
